/*
 * Copyright (c) Thorben Linneweber and others
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

using System;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;

using SoftFloat;

namespace Jitter2.LinearMath
{
	
	/// <summary>
	/// 3x3 matrix of 32 bit sfloat values in column major format.
	/// </summary>
	[StructLayout(LayoutKind.Explicit, Size = 36)]
	public struct JMatrix
	{
	    [FieldOffset(0)] public sfloat M11;
	    [FieldOffset(4)] public sfloat M21;
	    [FieldOffset(8)] public sfloat M31;
	    [FieldOffset(12)] public sfloat M12;
	    [FieldOffset(16)] public sfloat M22;
	    [FieldOffset(20)] public sfloat M32;
	    [FieldOffset(24)] public sfloat M13;
	    [FieldOffset(28)] public sfloat M23;
	    [FieldOffset(32)] public sfloat M33;
	
	    public static readonly JMatrix Identity;
	    public static readonly JMatrix Zero;
	
	    public override string ToString()
	    {
	        return $"({M11}, {M21}, {M31}, {M12}, {M22}, {M32}, {M13}, {M23}, {M33})";
	    }
	
	    static JMatrix()
	    {
	        Zero = new JMatrix();
	
	        Identity = new JMatrix
	        {
	            M11 = (sfloat)1.0f,
	            M22 = (sfloat)1.0f,
	            M33 = (sfloat)1.0f
	        };
	    }
	
	    public JMatrix(sfloat m11, sfloat m12, sfloat m13, sfloat m21, sfloat m22, sfloat m23, sfloat m31, sfloat m32, sfloat m33)
	    {
	        M11 = m11;
	        M12 = m12;
	        M13 = m13;
	        M21 = m21;
	        M22 = m22;
	        M23 = m23;
	        M31 = m31;
	        M32 = m32;
	        M33 = m33;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static JMatrix FromColumns(in NVector3 col1, in NVector3 col2, in NVector3 col3)
	    {
	        UnsafeEx.SkipInit(out JMatrix res);
	        res.UnsafeGet(0) = col1;
	        res.UnsafeGet(1) = col2;
	        res.UnsafeGet(2) = col3;
	        return res;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public unsafe ref NVector3 UnsafeGet(int index)
	    {
	        NVector3* ptr = (NVector3*)Unsafe.AsPointer(ref this);
	        return ref ptr[index];
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public unsafe NVector3 GetColumn(int index)
	    {
	        fixed (sfloat* ptr = &M11)
	        {
	            NVector3* vptr = (NVector3*)ptr;
	            return vptr[index];
	        }
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static JMatrix Multiply(in JMatrix matrix1, in JMatrix matrix2)
	    {
	        Multiply(matrix1, matrix2, out JMatrix result);
	        return result;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static JMatrix MultiplyTransposed(in JMatrix matrix1, in JMatrix matrix2)
	    {
	        MultiplyTransposed(matrix1, matrix2, out JMatrix result);
	        return result;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static JMatrix TransposedMultiply(in JMatrix matrix1, in JMatrix matrix2)
	    {
	        TransposedMultiply(matrix1, matrix2, out JMatrix result);
	        return result;
	    }
	
	    public static JMatrix CreateRotationMatrix(NVector3 axis, sfloat angle)
	    {
	        sfloat c = libm.Cos(angle / (sfloat)2.0f);
	        sfloat s = libm.Sin(angle / (sfloat)2.0f);
	        axis *= s;
	        NQuaternion jq = new(axis.X, axis.Y, axis.Z, c);
	        CreateFromQuaternion(in jq, out JMatrix result);
	        return result;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static void Multiply(in JMatrix matrix1, in JMatrix matrix2, out JMatrix result)
	    {
	        sfloat num0 = matrix1.M11 * matrix2.M11 + matrix1.M12 * matrix2.M21 + matrix1.M13 * matrix2.M31;
	        sfloat num1 = matrix1.M11 * matrix2.M12 + matrix1.M12 * matrix2.M22 + matrix1.M13 * matrix2.M32;
	        sfloat num2 = matrix1.M11 * matrix2.M13 + matrix1.M12 * matrix2.M23 + matrix1.M13 * matrix2.M33;
	        sfloat num3 = matrix1.M21 * matrix2.M11 + matrix1.M22 * matrix2.M21 + matrix1.M23 * matrix2.M31;
	        sfloat num4 = matrix1.M21 * matrix2.M12 + matrix1.M22 * matrix2.M22 + matrix1.M23 * matrix2.M32;
	        sfloat num5 = matrix1.M21 * matrix2.M13 + matrix1.M22 * matrix2.M23 + matrix1.M23 * matrix2.M33;
	        sfloat num6 = matrix1.M31 * matrix2.M11 + matrix1.M32 * matrix2.M21 + matrix1.M33 * matrix2.M31;
	        sfloat num7 = matrix1.M31 * matrix2.M12 + matrix1.M32 * matrix2.M22 + matrix1.M33 * matrix2.M32;
	        sfloat num8 = matrix1.M31 * matrix2.M13 + matrix1.M32 * matrix2.M23 + matrix1.M33 * matrix2.M33;
	
	        result.M11 = num0;
	        result.M12 = num1;
	        result.M13 = num2;
	        result.M21 = num3;
	        result.M22 = num4;
	        result.M23 = num5;
	        result.M31 = num6;
	        result.M32 = num7;
	        result.M33 = num8;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static JMatrix Add(JMatrix matrix1, JMatrix matrix2)
	    {
	        Add(matrix1, matrix2, out JMatrix result);
	        return result;
	    }
	
	    /// <summary>
	    /// Calculates matrix1 \times matrix2^\mathrm{T}.
	    /// </summary>
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static void MultiplyTransposed(in JMatrix matrix1, in JMatrix matrix2, out JMatrix result)
	    {
	        sfloat num0 = matrix1.M11 * matrix2.M11 + matrix1.M12 * matrix2.M12 + matrix1.M13 * matrix2.M13;
	        sfloat num1 = matrix1.M11 * matrix2.M21 + matrix1.M12 * matrix2.M22 + matrix1.M13 * matrix2.M23;
	        sfloat num2 = matrix1.M11 * matrix2.M31 + matrix1.M12 * matrix2.M32 + matrix1.M13 * matrix2.M33;
	        sfloat num3 = matrix1.M21 * matrix2.M11 + matrix1.M22 * matrix2.M12 + matrix1.M23 * matrix2.M13;
	        sfloat num4 = matrix1.M21 * matrix2.M21 + matrix1.M22 * matrix2.M22 + matrix1.M23 * matrix2.M23;
	        sfloat num5 = matrix1.M21 * matrix2.M31 + matrix1.M22 * matrix2.M32 + matrix1.M23 * matrix2.M33;
	        sfloat num6 = matrix1.M31 * matrix2.M11 + matrix1.M32 * matrix2.M12 + matrix1.M33 * matrix2.M13;
	        sfloat num7 = matrix1.M31 * matrix2.M21 + matrix1.M32 * matrix2.M22 + matrix1.M33 * matrix2.M23;
	        sfloat num8 = matrix1.M31 * matrix2.M31 + matrix1.M32 * matrix2.M32 + matrix1.M33 * matrix2.M33;
	
	        result.M11 = num0;
	        result.M12 = num1;
	        result.M13 = num2;
	        result.M21 = num3;
	        result.M22 = num4;
	        result.M23 = num5;
	        result.M31 = num6;
	        result.M32 = num7;
	        result.M33 = num8;
	    }
	
	    public static JMatrix CreateRotationX(sfloat radians)
	    {
	        JMatrix result = Identity;
	
	        sfloat c = (sfloat)libm.Cos(radians);
	        sfloat s = (sfloat)libm.Sin(radians);
	
	        // [  1  0  0  0 ]
	        // [  0  c -s  0 ]
	        // [  0  s  c  0 ]
	        // [  0  0  0  1 ]
	        result.M22 = c;
	        result.M23 = -s;
	        result.M32 = s;
	        result.M33 = c;
	
	        return result;
	    }
	
	    public static JMatrix CreateRotationY(sfloat radians)
	    {
	        JMatrix result = Identity;
	
	        sfloat c = (sfloat)libm.Cos(radians);
	        sfloat s = (sfloat)libm.Sin(radians);
	
	        // [  c  0  s  0 ]
	        // [  0  1  0  0 ]
	        // [ -s  0  c  0 ]
	        // [  0  0  0  1 ]
	        result.M11 = c;
	        result.M13 = s;
	        result.M31 = -s;
	        result.M33 = c;
	
	        return result;
	    }
	
	    public static JMatrix CreateRotationZ(sfloat radians)
	    {
	        JMatrix result = Identity;
	
	        sfloat c = (sfloat)libm.Cos(radians);
	        sfloat s = (sfloat)libm.Sin(radians);
	
	        // [  c -s  0  0 ]
	        // [  s  c  0  0 ]
	        // [  0  0  1  0 ]
	        // [  0  0  0  1 ]
	        result.M11 = c;
	        result.M12 = -s;
	        result.M21 = s;
	        result.M22 = c;
	
	        return result;
	    }
	
	    /// <summary>
	    /// Create a scaling matrix.
	    /// </summary>
	    /// <returns></returns>
	    public static JMatrix CreateScale(in NVector3 scale)
	    {
	        JMatrix result = Zero;
	
	        result.M11 = scale.X;
	        result.M22 = scale.Y;
	        result.M33 = scale.Z;
	
	        return result;
	    }
	
	    /// <summary>
	    /// Create a scaling matrix.
	    /// </summary>
	    /// <returns></returns>
	    public static JMatrix CreateScale(sfloat x, sfloat y, sfloat z)
	    {
	        return CreateScale(new NVector3(x, y, z));
	    }
	
	    /// <summary>
	    /// Calculates matrix1^\mathrm{T} \times matrix2.
	    /// </summary>
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static void TransposedMultiply(in JMatrix matrix1, in JMatrix matrix2, out JMatrix result)
	    {
	        sfloat num0 = matrix1.M11 * matrix2.M11 + matrix1.M21 * matrix2.M21 + matrix1.M31 * matrix2.M31;
	        sfloat num1 = matrix1.M11 * matrix2.M12 + matrix1.M21 * matrix2.M22 + matrix1.M31 * matrix2.M32;
	        sfloat num2 = matrix1.M11 * matrix2.M13 + matrix1.M21 * matrix2.M23 + matrix1.M31 * matrix2.M33;
	        sfloat num3 = matrix1.M12 * matrix2.M11 + matrix1.M22 * matrix2.M21 + matrix1.M32 * matrix2.M31;
	        sfloat num4 = matrix1.M12 * matrix2.M12 + matrix1.M22 * matrix2.M22 + matrix1.M32 * matrix2.M32;
	        sfloat num5 = matrix1.M12 * matrix2.M13 + matrix1.M22 * matrix2.M23 + matrix1.M32 * matrix2.M33;
	        sfloat num6 = matrix1.M13 * matrix2.M11 + matrix1.M23 * matrix2.M21 + matrix1.M33 * matrix2.M31;
	        sfloat num7 = matrix1.M13 * matrix2.M12 + matrix1.M23 * matrix2.M22 + matrix1.M33 * matrix2.M32;
	        sfloat num8 = matrix1.M13 * matrix2.M13 + matrix1.M23 * matrix2.M23 + matrix1.M33 * matrix2.M33;
	
	        result.M11 = num0;
	        result.M12 = num1;
	        result.M13 = num2;
	        result.M21 = num3;
	        result.M22 = num4;
	        result.M23 = num5;
	        result.M31 = num6;
	        result.M32 = num7;
	        result.M33 = num8;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static void Add(in JMatrix matrix1, in JMatrix matrix2, out JMatrix result)
	    {
	        result.M11 = matrix1.M11 + matrix2.M11;
	        result.M12 = matrix1.M12 + matrix2.M12;
	        result.M13 = matrix1.M13 + matrix2.M13;
	        result.M21 = matrix1.M21 + matrix2.M21;
	        result.M22 = matrix1.M22 + matrix2.M22;
	        result.M23 = matrix1.M23 + matrix2.M23;
	        result.M31 = matrix1.M31 + matrix2.M31;
	        result.M32 = matrix1.M32 + matrix2.M32;
	        result.M33 = matrix1.M33 + matrix2.M33;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static void Subtract(in JMatrix matrix1, in JMatrix matrix2, out JMatrix result)
	    {
	        result.M11 = matrix1.M11 - matrix2.M11;
	        result.M12 = matrix1.M12 - matrix2.M12;
	        result.M13 = matrix1.M13 - matrix2.M13;
	        result.M21 = matrix1.M21 - matrix2.M21;
	        result.M22 = matrix1.M22 - matrix2.M22;
	        result.M23 = matrix1.M23 - matrix2.M23;
	        result.M31 = matrix1.M31 - matrix2.M31;
	        result.M32 = matrix1.M32 - matrix2.M32;
	        result.M33 = matrix1.M33 - matrix2.M33;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public readonly sfloat Determinant()
	    {
	        return M11 * M22 * M33 + M12 * M23 * M31 + M13 * M21 * M32 -
	               M31 * M22 * M13 - M32 * M23 * M11 - M33 * M21 * M12;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static bool Inverse(in JMatrix matrix, out JMatrix result)
	    {
	        sfloat idet = (sfloat)1.0f / matrix.Determinant();
	
	        if (!sfloat.IsNormal(idet))
	        {
	            result = new JMatrix();
	            return false;
	        }
	
	        sfloat num11 = matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32;
	        sfloat num12 = matrix.M13 * matrix.M32 - matrix.M12 * matrix.M33;
	        sfloat num13 = matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13;
	
	        sfloat num21 = matrix.M23 * matrix.M31 - matrix.M33 * matrix.M21;
	        sfloat num22 = matrix.M11 * matrix.M33 - matrix.M31 * matrix.M13;
	        sfloat num23 = matrix.M13 * matrix.M21 - matrix.M23 * matrix.M11;
	
	        sfloat num31 = matrix.M21 * matrix.M32 - matrix.M31 * matrix.M22;
	        sfloat num32 = matrix.M12 * matrix.M31 - matrix.M32 * matrix.M11;
	        sfloat num33 = matrix.M11 * matrix.M22 - matrix.M21 * matrix.M12;
	
	        result.M11 = num11 * idet;
	        result.M12 = num12 * idet;
	        result.M13 = num13 * idet;
	        result.M21 = num21 * idet;
	        result.M22 = num22 * idet;
	        result.M23 = num23 * idet;
	        result.M31 = num31 * idet;
	        result.M32 = num32 * idet;
	        result.M33 = num33 * idet;
	
	        return true;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static JMatrix Multiply(JMatrix matrix1, sfloat scaleFactor)
	    {
	        Multiply(in matrix1, scaleFactor, out JMatrix result);
	        return result;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static void Multiply(in JMatrix matrix1, sfloat scaleFactor, out JMatrix result)
	    {
	        sfloat num = scaleFactor;
	        result.M11 = matrix1.M11 * num;
	        result.M12 = matrix1.M12 * num;
	        result.M13 = matrix1.M13 * num;
	        result.M21 = matrix1.M21 * num;
	        result.M22 = matrix1.M22 * num;
	        result.M23 = matrix1.M23 * num;
	        result.M31 = matrix1.M31 * num;
	        result.M32 = matrix1.M32 * num;
	        result.M33 = matrix1.M33 * num;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static JMatrix CreateFromQuaternion(NQuaternion quaternion)
	    {
	        CreateFromQuaternion(quaternion, out JMatrix result);
	        return result;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static void Absolute(in JMatrix matrix, out JMatrix result)
	    {
	        result.M11 = libm.Abs(matrix.M11);
	        result.M12 = libm.Abs(matrix.M12);
	        result.M13 = libm.Abs(matrix.M13);
	        result.M21 = libm.Abs(matrix.M21);
	        result.M22 = libm.Abs(matrix.M22);
	        result.M23 = libm.Abs(matrix.M23);
	        result.M31 = libm.Abs(matrix.M31);
	        result.M32 = libm.Abs(matrix.M32);
	        result.M33 = libm.Abs(matrix.M33);
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static void CreateFromQuaternion(in NQuaternion quaternion, out JMatrix result)
	    {
	        sfloat r = quaternion.W;
	        sfloat i = quaternion.X;
	        sfloat j = quaternion.Y;
	        sfloat k = quaternion.Z;
	
	        result.M11 = (sfloat)1.0f - (sfloat)2.0f * (j * j + k * k);
	        result.M12 = (sfloat)2.0f * (i * j - k * r);
	        result.M13 = (sfloat)2.0f * (i * k + j * r);
	        result.M21 = (sfloat)2.0f * (i * j + k * r);
	        result.M22 = (sfloat)1.0f - (sfloat)2.0f * (i * i + k * k);
	        result.M23 = (sfloat)2.0f * (j * k - i * r);
	        result.M31 = (sfloat)2.0f * (i * k - j * r);
	        result.M32 = (sfloat)2.0f * (j * k + i * r);
	        result.M33 = (sfloat)1.0f - (sfloat)2.0f * (i * i + j * j);
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static JMatrix Transpose(in JMatrix matrix)
	    {
	        Transpose(in matrix, out JMatrix result);
	        return result;
	    }
	
	    /// <summary>
	    /// Returns JMatrix(0, -vec.Z, vec.Y, vec.Z, 0, -vec.X, -vec.Y, vec.X, 0)-
	    /// </summary>
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static JMatrix CreateCrossProduct(in NVector3 vec)
	    {
	        return new JMatrix(0, -vec.Z, vec.Y, vec.Z, 0, -vec.X, -vec.Y, vec.X, 0);
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    private static void Transpose(in JMatrix matrix, out JMatrix result)
	    {
	        result.M11 = matrix.M11;
	        result.M12 = matrix.M21;
	        result.M13 = matrix.M31;
	        result.M21 = matrix.M12;
	        result.M22 = matrix.M22;
	        result.M23 = matrix.M32;
	        result.M31 = matrix.M13;
	        result.M32 = matrix.M23;
	        result.M33 = matrix.M33;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static JMatrix operator *(in JMatrix matrix1, in JMatrix matrix2)
	    {
	        JMatrix result;
	        result.M11 = matrix1.M11 * matrix2.M11 + matrix1.M12 * matrix2.M21 + matrix1.M13 * matrix2.M31;
	        result.M12 = matrix1.M11 * matrix2.M12 + matrix1.M12 * matrix2.M22 + matrix1.M13 * matrix2.M32;
	        result.M13 = matrix1.M11 * matrix2.M13 + matrix1.M12 * matrix2.M23 + matrix1.M13 * matrix2.M33;
	        result.M21 = matrix1.M21 * matrix2.M11 + matrix1.M22 * matrix2.M21 + matrix1.M23 * matrix2.M31;
	        result.M22 = matrix1.M21 * matrix2.M12 + matrix1.M22 * matrix2.M22 + matrix1.M23 * matrix2.M32;
	        result.M23 = matrix1.M21 * matrix2.M13 + matrix1.M22 * matrix2.M23 + matrix1.M23 * matrix2.M33;
	        result.M31 = matrix1.M31 * matrix2.M11 + matrix1.M32 * matrix2.M21 + matrix1.M33 * matrix2.M31;
	        result.M32 = matrix1.M31 * matrix2.M12 + matrix1.M32 * matrix2.M22 + matrix1.M33 * matrix2.M32;
	        result.M33 = matrix1.M31 * matrix2.M13 + matrix1.M32 * matrix2.M23 + matrix1.M33 * matrix2.M33;
	        return result;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public sfloat Trace()
	    {
	        return M11 + M22 + M33;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static JMatrix operator *(sfloat factor, in JMatrix matrix)
	    {
	        Multiply(matrix, factor, out JMatrix result);
	        return result;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static JMatrix operator *(in JMatrix matrix, sfloat factor)
	    {
	        Multiply(matrix, factor, out JMatrix result);
	        return result;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static JMatrix operator +(in JMatrix value1, in JMatrix value2)
	    {
	        Add(value1, value2, out JMatrix result);
	        return result;
	    }
	
	    [MethodImpl(MethodImplOptions.AggressiveInlining)]
	    public static JMatrix operator -(in JMatrix value1, in JMatrix value2)
	    {
	        Subtract(value1, value2, out JMatrix result);
	        return result;
	    }
	}
}
